#include "./robot_arm.h"
#include "../rtos/thread.h"
#include "../driver/robot_arm1.h"
#include "../driver/robot_arm2.h"
#include "../rtos/event_flags.h"

namespace service::robot_arm {

using namespace utils;
using namespace driver;

utils::error_t reset() {
    utils::error_t err = robotArm1->run(RobotArm1::ORIGIN_POSITION);
    if (err) {
        return err;
    }
    err = robotArm2->run(RobotArm2::ORIGIN_POSITION);
    return err;
}

utils::error_t run(uint16_t p1, uint16_t p2) {
    if ((p1 == RobotArm1::ORIGIN_POSITION) && (p2 == RobotArm2::ORIGIN_POSITION)) {
        return reset();
    }
    utils::error_t err = robotArm2->run(p2);
    if (err) {
        return err;
    }
    return robotArm1->run(p1);
}

}